#include"poseSolver.h"
#define SHOWDIS
//#define TEST1
void poseSolver::init(){
#ifdef TEST1
  double cameraD[3][3] = { { 696.2415644249758, 0, 292.4934267612221, },
                             { 0, 695.8978292876928, 245.7262723076008, },
                             { 0 ,0 , 1.0000 } };   //通过张氏标定法得到的的单目相机内参
                                                    /*[672.5922798370848, 0, 337.132948890464;
 0, 680.0782236083621, 290.6690157155584;
 0, 0, 1]*/
    double distC[5] = {  -0.07237108290032994, 0.2749124813770291, -0.003386433488518278, -0.000453528346320908, -0.2629695436000922};//通过张氏标定法获取的相机畸变参数
                                                    /*[-0.4430247649796284, 9.412991720562699, 0.006571016681536355, 0.01661428891908618, -55.80088956515684]
*/

//转到矩阵中
    Mat c(3, 3, cv::DataType<double>::type, cameraD);
    Mat d(5, 1, cv::DataType<double>::type, distC);
    c.copyTo(cameraMatrix);
    d.copyTo(distCoeffs);
    
#else 
    double cameraD[3][3] = { { 1.2853517927598091e+03, 0., },
                             { 0.,1.2792339468697937e+03, 2.3929354061292258e+02, },
                             { 0 ,0 , 1.0000 } };   //通过张氏标定法得到的的单目相机内参
                                                    /*[ 1.2853517927598091e+03, 0., 3.1944768628958542e+02, 0.,
 1.2792339468697937e+03, 2.3929354061292258e+02, 0., 0., 1. ]*/
    double distC[5] = { -6.3687295852461456e-01, -1.9748008790347320e+00,3.0970703651800782e-02, 2.1944646842516919e-03, 0. };//通过张氏标定法获取的相机畸变参数
                                                    /*[ -6.3687295852461456e-01, -1.9748008790347320e+00,3.0970703651800782e-02, 2.1944646842516919e-03, 0. ]
*/

//转到矩阵中
    Mat c(3, 3, cv::DataType<double>::type, cameraD);
    Mat d(5, 1, cv::DataType<double>::type, distC);
    c.copyTo(cameraMatrix);
    d.copyTo(distCoeffs);
#endif
    return ;
}
void poseSolver::get2DPoints(Point2f vertex[4]) {
    Point2f firstP, fourthP, secondP, thirdP;
    sort( vertex,  vertex + 4, [](const Point2f& p1, const Point2f& p2) { return p1.x < p2.x; });//从4个点的第一个到最后一个进行排序
    if ( vertex[0].y <  vertex[1].y) {

        firstP =  vertex[0];
        fourthP =  vertex[1];
    }
    else {

        firstP =  vertex[1];
        fourthP =  vertex[0];
    }
    if ( vertex[2].y <  vertex[3].y) {

        secondP =  vertex[2];
        thirdP =  vertex[3];
    }
    else {

        secondP =  vertex[3];
        thirdP =  vertex[2];
    }

    target2D.clear();//重要,因为这是实时测量
    target2D.push_back(firstP);
    target2D.push_back(secondP);
    target2D.push_back(thirdP);
    target2D.push_back(fourthP);
    return;
}
void poseSolver::Generate3DPoints(int angle_solver_algorithm){
    target3D.clear();

    if(angle_solver_algorithm==0){
        double perL = armor_length / 2;
        double perH = armor_height / 2;
        double x, y, z;
        x = -perL; y = perH; z = 0;
        target3D.push_back(cv::Point3f(x, y, z));

        x = perL; y = perH; z = 0;
        target3D.push_back(cv::Point3f(x, y, z));

        x = perL;    y = -perH; z = 0;
        target3D.push_back(cv::Point3f(x, y, z));

        x = -perL; y = -perH; z = 0;
        target3D.push_back(cv::Point3f(x, y, z));


    }
    return ;
}
void poseSolver::solve_for_armor(Point2f vertex[],int angle_solver_algorithm  ){
    unqualified_dis=0;
    if(firstrun){
         init();
         Generate3DPoints(angle_solver_algorithm);
         firstrun=0;
    }
   
    get2DPoints(vertex);
    
    solvePnP(target3D,target2D,cameraMatrix,distCoeffs,_rVec,_tVec);
    _tVec.at<double>(1, 0) += Y_DISTANCE_BETWEEN_GUN_AND_CAM;
	_xErr = atan(_tVec.at<double>(0, 0) / _tVec.at<double>(2, 0)) / 2 / CV_PI * 360;
	_yErr = -(atan(_tVec.at<double>(1, 0) / _tVec.at<double>(2, 0)) / 2 / CV_PI * 360);
	_euclideanDistance = sqrt(_tVec.at<double>(0, 0)*_tVec.at<double>(0, 0) + _tVec.at<double>(1, 0)*_tVec.at<double>(1, 0) + _tVec.at<double>(2, 0)* _tVec.at<double>(2, 0));
#ifdef SHOWDIS
     cout<<_euclideanDistance<<"CM"<<endl;
#endif
     //too far or too close are not qualified.
     if(_euclideanDistance>400||_euclideanDistance<30){
         unqualified_dis=1;
     }
    //compensateGravity();
    return ;
}
void poseSolver::compensateGravity()
{
	const auto& theta_p_prime = _yErr / 180 * CV_PI;
	const auto& d_prime = _euclideanDistance;
	const auto& v = bullet_speed;
	const auto theta_p_prime2 = atan((sin(theta_p_prime) - 0.5*9.8*d_prime / pow(v, 2)) / cos(theta_p_prime));
	_yErr = theta_p_prime2 / CV_PI * 180;
    return ;
}
bool poseSolver::wrong_dis(){
    if(unqualified_dis){
        return 1;
    }
    else return 0;
}
double poseSolver::get_yaw(){
    return _xErr;
}
double poseSolver::get_pitch(){
    return _yErr;
}

